#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,                sensorI2CCustom9V)
#pragma config(Sensor, S3,     SMUX_1,              sensorI2CCustom)
#pragma config(Sensor, S4,     SMUX_2,              sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     e,             tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     a,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     b,             tmotorNormal, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     c,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     f,             tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     d,             tmotorNormal, PIDControl, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    frontarm,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    backarm,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    dispencer,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_4,    Irpullup,             tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    Irfront,              tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    Irback,               tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "HTEOPD-driver.h"
#include "SensorLog.c"

#define EOPD_R msensor_S3_4
#define EOPD_L msensor_S3_3

#define LOW_PASS_EOPD_ALPHA 0.35

task main() {
  HTSMUXinit();
  HTSMUXscanPorts (SMUX_1);
  HTSMUXscanPorts (SMUX_2);
  HTEOPDsetLongRange(EOPD_R);
  HTEOPDsetLongRange(EOPD_L);
/*  int indx = 0;
  SensorLogBegin("idx,", "Left, ", "Right, ", "NULL");
  while (indx < 1400) {
    SensorLog(indx, HTEOPDreadRaw(EOPD_L), HTEOPDreadRaw(EOPD_R), 0);
    indx++;
  }
  sensorLogEnd();
  */
  int lft_value = HTEOPDreadRaw(EOPD_L);
  int rgt_value = HTEOPDreadRaw(EOPD_R);


  while (true) {
    lft_value = lft_value * (1-LOW_PASS_EOPD_ALPHA) + LOW_PASS_EOPD_ALPHA * HTEOPDreadRaw(EOPD_L);
    rgt_value = rgt_value * (1-LOW_PASS_EOPD_ALPHA) + LOW_PASS_EOPD_ALPHA * HTEOPDreadraw(EOPD_R);
    nxtDisplayTextLine(2, "Left Val: %4d", lft_value);
    nxtDisplayTextLine(3, "Right Val: %4d", rgt_value);

  }

}
